Autonomous nap-of-the-earth (anoe) flight path planning for manned and unmanned rotorcraft

ABSTRACT

A flight path planning approach may be deterministic and guarantee a safe, quasi-optimal path. A plurality of three-dimensional voxels may be determined as cells of a rectangular grid. The cells may have a predetermined length and width. A shortest safe path through the grid graph may be calculated from a local start to a local goal defined as points on a nominal global path. Geometric smoothing may be performed on the basis line from the local start to the local goal to generate a smooth three-dimensional trajectory that can be followed by a given rotorcraft. Dynamic smoothing may be performed on the three-dimensional trajectory to provide a maximum possible speed profile over a path defined by the dynamic smoothing. The three dimensional path information may be provided to an autopilot, which may then control the rotorcraft to fly along the defined path.

FIELD

The present invention generally pertains to vehicle flight planning, andmore specifically, to path and trajectory planning for manned andunmanned rotorcraft based on sensor information that guarantees safetyof the rotorcraft and its surroundings.

BACKGROUND

The lack of adequate “detect and avoid” and “sense and avoid”capabilities has been a significant technological barrier that haslimited the progress of the adoption of unmanned aerial systems (UAS)into the National Airspace System (NAS). Because there are no pilotsonboard such systems, a UAS cannot comply with the “see and avoid”requirements that underpin operational safety in the NAS. There iscurrently a lack of a mature UAS technology capable of automaticallydetecting other aircraft operating in nearby airspace and successfullymaneuvering to avoid them.

A rotorcraft, such as a helicopter, may employ different flight pathssuch as paths 100 illustrated in FIG. 1. In low level flight 110, therotorcraft flies relatively low to the ground, but flies along astraight path. In contour flight 120, the rotorcraft maintains arelatively constant height above the ground, making adjustments based onterrain contour. In Nap-Of-the-Earth flight 130, the rotorcraft fliesvery close to the ground, adjusting height to avoid hitting the ground,avoiding obstacles, and potentially using obstacles and terrain folds ascover.

Both civilian and military types of NOE missions may involve flight veryclose to the ground, requiring avoidance of obstacles such as trees,poles, wires, and buildings. For military missions, using terrainfeatures as masking from enemy sensors may be beneficial. This NOEflight mode maximizes survivability for military missions because mostanti-aircraft (AA) weapons require line-of-sight (LOS) contact for someperiod of time in order to compute a firing solution. For commercial andcivilian operations (e.g., medevac, package delivery, firefighting,etc.), it is often necessary to fly NOE in urban or forested areas richin ground obstacles.

In manned NOE flight, there is little margin for error for the pilot. Atan altitude of 6 meters, for example, a helicopter flying at just 40kilometers per hour can hit the ground in just half a second. The pilothas to navigate to the target while deciding how to avoid obstaclesdetected at relatively close ranges of less than 1 km. In the presenceof ground-based threats, the pilot also has to remain masked from enemyradar by navigating areas protected by terrain folds. For night and/orbad weather operations, the pilot will generally need to wear displaygoggles or an eyepiece displaying information from external sensors,such as forward-looking infrared (FLIR), etc. These devices haverelatively narrow fields of view and inherent time delays on headtracking motions, which may result in motion sickness for rapid headmotions. In medevac missions, the pilot will also look for the injuredperson's location and lower the transport basket or other gear attachedto a steel cable, while keeping the cable free of tangles orobstructions. As a consequence, NOE operations impose extremely highworkloads on the flight crew, especially the pilot.

Some conventional optimal NOE trajectory planning solutions use a linearcombination of flight time and terrain masking as the performance index.The trajectory masking was assumed to be accomplished if an integralproportional to the helicopter altitude above the terrain was minimizedSuch solutions required the first and second partial derivatives of theterrain, which could be computed using a cubic polynomialparameterization of the digital terrain map (DTM) elevation data.However, for terrain data obtained in-flight from sensor readings, thesederivatives would be very noisy and inexact. The Autonomous HelicopterSystem (AHS) had a three section knowledge-based architecture: vision,planning, and control. Vision provided the local and global sceneanalysis that is symbolically represented and passed to planning as theinitial route planning constraints. Planning generated a task dependentpath for the vehicle to traverse, which assures maximum mission systemsuccess as well as safety. Control validated the path and eitherexecuted the given route or fed back to previous sections in order toresolve conflicts.

The IBM Advanced Flight Path Management System displayed pilot cues forsmooth tracking of the desired flight path. It consisted of an onboardroute planner that computed the optimum flight path, minimizingexposure, a covert terrain following function based on an advanced lowaltitude techniques (ADLAT) terrain following algorithm, a guidancefunction that determined path errors between the present position anddesired flight path, and a flight director function that computed pilotcommands for aided manual flight. This approach integrated output fromFLIR, multimode radar (MMR), a global positioning system (GPS), a highaccuracy inertial navigation system (INS), Doppler radar, air datasensors, and an attitude heading reference system (AHRS).

The NOE path planning problem has been divided into “near field” and“far field” problems. Near field is associated with flight in the nextseveral seconds, in the order of tens of seconds at most. Far fieldrefers to a longer time interval. For example, this time interval may beon the order of tens of minutes. The far field solution as embodied inwaypoints is determined during mission planning, and is one of theinputs to the NOE calculation. The near field path planning is carriedout over a rectangular region divided into cells, and is based onreal-time optimization of a performance measure. The NOE planning systemof some conventional approaches is divided into far field planning(i.e., generating the global path between waypoints based on terrain,vehicle resources, and threat information using simulated annealing ordynamic programming), mid-field planning (i.e., refining the flightroute generated by the far-field planner for a short duration ahead byreferring to more detailed terrain information, possible even terrainmasking from threats) and near field obstacle avoidance planning (i.e.,maneuvering, based on obstacle detection sensor data, around the nominalpath generated by the mid-field planner by a two dimensional range mapof range as a function of azimuthal look angle).

In NOE mode, the vehicle flies as close to the ground as vegetation andobstacles safely permit, generally following the contours of theterrain, and thus minimizing exposure to potential threats. The terrainthrough which the vehicle must fly is considered “hostile” in the sensethat there are enemy emplacements with LOS-type sensors such as laserand radar, whose positions are not known. Some information on the enemysensors and weapons characteristics, such as lock-and-launch times, isknown a priori. Also, initial information is available concerning someof the locations and movements of these threats, but the threats areexpected to move and/or become active or dormant, and the total numberof threats is unknown.

Path planning on hostile terrain has been extensively studied for groundand airborne vehicles. Although there are differences between theconventional solutions to this problem, there has essentially been aconvergence to two solution methodologies for planning a path betweentwo points on the terrain. One methodology involves the use of digitalterrain elevation data (DTED). DTED subdivides the terrain into “terraincells” by a homogeneous grid in the horizontal plane. Height informationis stored and updated by sensor information for each cell. This data isthen processed in order to assign some “weight” to each terrain cell,and then some “cost” function is evaluated to find the “cheapest” (i.e.,shortest) path by A* or similar graph search algorithms. Alternatively,the DTED database is processed to create a 2-D polygonal obstacle map,and then a graph-like database, or a range map, is generated that can besearched by a heuristic algorithm. The other methodology involvesmodeling of the terrain by a smoothing function and applying optimalcontrol theory for trajectory planning. This methodology is notpractical for in-flight re-planning mainly because the terrain dataprovided by the sensors is very noisy.

In the DTED approach, the path generation process first generates thefootprint of the flight path on the x-y plane, and then generates thevertical flight path along the footprint. This two-stage approach topath generation requires significantly less complex algorithms thanother approaches. The horizontal (i.e., lateral) path may beapproximated by straight lines that are made as long as possible toeliminate “digitization bias,” which is a product of requiring paths tostay on the grid graph. The lines may be connected by tangent arcs thatwould correspond to coordinated aircraft turns without sideslip. Thevertical plane is defined by the s (along the x-y path footprint) and z(absolute height) coordinates. The vertical path may be approximated bylines and arcs in the s-z plane. Normal acceleration in the verticaldimension can be related to an effective “vertical” turn radius.

The terrain profile may be generated along the horizontal path. Itsparallel profile at the nominal flight height becomes the basis forvertical smoothing. Since helicopters have more strict limitations innegative accelerations than in vertical accelerations, the vertical pathmay be designed to minimize sudden dives (i.e., push-down maneuvers),following the terrain just close enough to avoid this issue. Severalconventional algorithms have been proposed to eliminate push-downs.These include the branch-and-prune technique, optimal control, constantacceleration parabolas, and commanded altitude rate for present groundspeed.

The dynamic constraints of the helicopter may be stored in the system asmaximum acceleration versus distance, deceleration (i.e., flare) versusdistance, and slope versus speed curves (e.g., lateral, longitudinal andvertical maximum accelerations, pitch and roll limits, and maximumangular rates). These constraints are specific to the helicopter and aredetermined by flight testing or estimated by simulation. The parametersalso vary with weather conditions and height.

Standard approaches to modeling the terrain for the obstacle avoidanceproblem, where the plane is subdivided into obstacles or nonobstacles,include subdivision of the plane into small regular “pixels,”hierarchical subdivision (e.g. quadtree), polygonal description ofobstacles, description of obstacles by circles, ellipses, or othernon-polygonal objects. The standard approaches may also includedescription of obstacles by splinegons (i.e., closed curved figures),and decomposition of free space into generalized cylinders. Shortestpaths according to Euclidean metrics will be “taut string” paths, likethreading a string from start to a goal among obstacles and pulling thestring “taut.”

The worst-case running time of the shortest path algorithm on a regulargrid with n pixels is O(n log n). However, the path is not the shortestaccording to the Euclidean metric due to “digitization bias” of the8-connected grid. For maps given in the form of a list of polygonalobstacles, the shortest path can be found by building a visibility graphof the obstacle space or by building a shortest path map. Thecomputational bottleneck is in the construction of the graph, requiringtime O(n²) for most graphs and O(n log n) for sparse graphs. The nodesof the visibility graph are the vertices of the obstacles and the edgesdo not cross the interior of any obstacle. The graph can be searchedusing A* or Dijkstra's algorithm. The shortest path map is a subdivisionof the plane into regions that are loci of all goal points whoseshortest paths to start pass through the same sequence of obstaclevertices. A shortest path is found by location of the goal point in itssubdivision, done in time O(n log n) and then backtracking an optimalpath (i.e., the “continuous Dijkstra” algorithm). In conclusion, thebest terrain model for the NOE in-flight path planning problem is aregular grid, since the height information on each “pixel” (i.e.,terrain cell) changes according to real-time sensor data. The problem of“digitization bias” is alleviated by path smoothing.

The problem of finding the shortest path for a vehicle constrained on anon-planar surface is the geodesic problem. In this case, therepresentation of the surface is in three dimensions: (1)polyhedral—given by faces, edges, and vertices with their correspondingadjacency relationships; (2) contour lines—similar to geological terrainmaps, given by iso-elevation curves; and (3) elevation array—atwo-dimensional array of numbers representing the altitude at each gridpoint. Digital terrain databases of the form compiled by the DefenseMapping Agency (also called Digital Terrain Maps (DTMs)) fall into thiscategory.

The solution for a map given by elevation array is analogous to theplanar case. For the polyhedral representation, however, the generalproblem is proved to be NP-hard. For convex polyhedra, the shortest pathproblem can be solved with complexity O(n² log n) using shortest pathmaps.

It is assumed that the map of the terrain represents a set of regions inthe plane, each having an associated weight or cost c. This weightspecifies the “cost per unit distance” of a vehicle traveling in thatregion. The objective is to find a path from start to goal in the planethat minimizes total cost according to this weighted Euclidean metric.This weighted region problem is a generalization of the obstacleavoidance problem where the weight for “free space” is 1 and for an“obstacle” is +∞. In military applications, there might be regionsexposed to threats having an associated LOS where traveling incurs highcosts associated with the danger.

There are two basic types of map representations: regular tessellations(e.g., grids of pixels, or quadtrees) and straight line planarsubdivisions. The regular grid of pixels representation is frequentlyused for terrain data. A set of arrays is provided, each array storinginformation about some particular aspect (ground cover, land usage,hydrography data, man-made features, and traversability indices). DMAdata pixels are usually squares 5, 12.5, or 100 meters on a side. If theterrain map is given as a grid of weighted pixels, the previouslypresented solution for digitized obstacle maps applies. However, thisapproach has two main problems: (1) fine grids are needed for goodfeature representation, which requires a large amount of memory; and (2)digitization bias is caused by confining movements to 8 orientations.Grid graphs of multiple resolution and a hierarchical algorithm can beused to alleviate these problems.

A proposed Spatial Database Management System (SDBMS) consisting of afully integrated, object-oriented and multiple resolution true spatialrepresentation was shown to significantly enhance problem solvingefficiency in information fusion, target recognition, situationassessment, autonomous vehicle control, and planning systems. The hybridrepresentation consists of: (1) pyramidal multiple resolution,object-oriented spatial representation; (2) region quadtrees; and (3) anobject-oriented database. In the pyramidal multiple resolution,object-oriented spatial representation, each pyramid cell is an objectthat includes a concise semantic description of a particular geographicregion.

Region quadtrees are regular, recursive decompositions of Euclideanspace into equal-sized quadrants, down to a minimumapplication-dependent resolution. The nodes are object-orientedrepresentations of spatial regions and the arcs define the geographicdecomposition. General properties of features are inherited from below.Nearly all quadtree-related algorithms require only integer arithmetic,and many can be implemented using a simple table lookup. In case ofterrain without overhangs, linear oct-trees, where a 2n*2n*2n array issubdivided into octants, can be used for storage of three-dimensionalobjects. The storage overhead for pointers is overcome by encoding thelevel of subdivision within each node. This method is used to representnon-layered terrain without overhangs. Grid points within a fixed bandof elevation can be aggregated to a single block according to theirplanar position (i.e., a “terrain oct-tree”). There are two majoradvantages of the use of terrain oct-trees: (1) transformations betweenterrain coordinates and the address of the corresponding node in theoct-tree are straightforward; and (2) the redundant nodes below thesurface can be discarded, significantly reducing the size of theoct-tree. This can be accomplished by projection of the terrain surface.

The object-oriented database includes point features, such as bridges,road intersections, and buildings, within a maximum resolution pyramidcell having a straightforward mapping to the semantic objectrepresentation. For the inverse mapping, the semantic object framepoints to the appropriate minimum resolution pyramid node(s). Within amaximum resolution pyramid node, each line feature is represented as anobject whose attributes are the vertices of a piecewise linear functionstored in a quadtree node-offset form. For lines and regions, therelationship between the spatial and object-oriented representations arenot 1:1. An explicit pointer links the line segment to the specificsemantic object. The inverse link points to the one or more pyramidnodes that contain the line feature. The pyramid provides a hierarchicalspatial indexing scheme to generate generic regions, where pyramid nodesup to and including the minimum enclosing quadtree block point directlyto the semantic object representation.

The proposed SDBMS may be highly distributed, run by multipleprocessors, relaxing the execution speed and memory requirements foreach individual processor. The regions are simple polygons if the map ismodeled as a straight line planar subdivision. This polygonal terrainapproach requires significantly less storage capacity than the regulargrids. One approach to the solution of the weighted region problem is tosubdivide the terrain into regions, place a node at the centroid of eachregion, connect the nodes of adjacent regions with arcs (weighted bytheir length) and search this graph for shortest paths. However, thisapproach produces paths that are far from optimal.

Another approach uses the continuous Dijkstra paradigm and has acomplexity of O(n⁷ L), where L is the number of bits needed to representthe problem data, and ε-optimal paths are computed, which are longerthan the optimal paths by ε percent. This approach assumes straight-lineplanar subdivision, where each region is a closed polygon with anassociated weight per unit length travelled in its midst and differentweights per unit length travelled on the edges. The shortest pathsgenerated by this approach are piecewise linear, bending only where theypass through a vertex or cross an edge. The bending angle obeys Snell'sLaw of Refraction, seeking optimality like light travelling throughregions with different indexes of refraction. An optimal path can travelalong an edge whose cost is less than its neighboring regions.

Consider the special case in which all the weights on faces and edgesare either 0 (travel over them can be done at high speeds), 1 (travelcan be accomplished at limited speeds only) or +∞ (blocking obstacles).This applies to the maximum concealment problem, where the regionsvisible to enemy threats have a finite cost, obstacles have infinitecost and other areas are inexpensive. An obstacle avoiding path betweentwo points is desired, which maximizes the concealment and minimizesexposure. Some special cases can be solved with polynomial-timealgorithms.

The least risk watchman route problem is a shortest path problem whereall the terrain is required to have had LOS contact with the travelingvehicle at some time, while minimizing the exposure to threats. For thecase of rectilinear polygons with n sides, there is an algorithm thatsolves it with complexity O(k²n³) for k threats. The special case ofweights {0, 1, +∞} can be solved in polynomial time by shrinking thezero-cost regions to single nodes, building an “extended visibilitygraph” and searching for the shortest path using Dijkstra or A*.

The special case of navigation on homogeneous terrain with a network ofroads is a weighted region problem where all of the regions have thesame cost and the edge costs are different. If the shortest paths areassumed to enter a road segment either at one of the endpoints or byhitting at the critical angle of incidence defined by the ratio of theedge's weight to that of the surrounding terrain, then a discrete“critical graph” can be searched for shortest paths. The criticalgraph's construction and the search can be done in time O(n²), where nis the number of road edges. Generalization to the case ofmultiple-source points yields Voronoi diagrams. Uniform cost per regioncan be generalized to a cost function, and the boundaries betweenregions can be allowed to be curved. A good map representation forrealistic vehicle navigation should integrate sensor-based informationacquired en-route, and the planner should be able to modify the originalplanned path according to changing conditions.

Prior approaches to ANOE flight planning are expensive and complex,requiring advanced processing and components. This is not practical formany vehicles. Also, prior approaches cannot produce a guaranteedsolution. Accordingly, an improved approach to ANOE path planning may bebeneficial.

SUMMARY

Certain embodiments of the present invention may be implemented andprovide solutions to the problems and needs in the art that have not yetbeen fully solved by conventional path planning approaches and systems.For example, some embodiments of the present invention guarantee a safe,quasi-optimal path. A shortest safe path through a rectangular gridgraph may be calculated from a local start to a local goal defined aspoints on a nominal global path. Geometric smoothing may be performed onthe basis line from the local start to the local goal to generate asmooth three-dimensional trajectory that can be followed by a givenrotorcraft. Dynamic smoothing may be performed on the three-dimensionaltrajectory to provide a maximum possible speed profile over a pathdefined by the dynamic smoothing.

In one embodiment of the present invention, a computer-implementedmethod includes determining a-priori, by a computing system, a pluralityof three-dimensional voxels as cells of a rectangular grid. The cellshave a predetermined length and width. The computer-implemented methodalso includes calculating, by the computing system, a shortest safe paththrough the grid graph from a local start to a local goal defined aspoints on a nominal global path. The computer-implemented method furtherincludes performing, by the computing system, geometric smoothing on abasis line from the local start to the local goal to generate a smooththree-dimensional trajectory that can be followed by a given rotorcraftand performing dynamic smoothing on the three-dimensional trajectory, bythe computing system, to provide a maximum possible speed profile over apath defined by the dynamic smoothing Additionally, thecomputer-implemented method includes providing a nominal path and speedprofile, by the computing system, to be used by an autopilot to controlthe rotorcraft to fly along the defined path.

In another embodiment, a computing system includes memory storingexecutable computer program code and at least one processor configuredto execute the computer program code. The at least one processor isconfigured to determine a basis line through points at centers of8-connected cells on a modified Dijkstra shortest path through a gridgraph from a start point to an end point. The at least one processor isalso configured to determine a polygonal line defining a minimumpossible number of straight lines between points on the basis line, thepolygonal line connecting the start point and the end point anddetermine boundary lines parallel to the polygonal line defining a safecorridor. The boundary lines of the corridor are determined bydimensions and clearances for a rotorcraft. The at least one processoris further configured to determine a connecting arc for each change indirection to generate a horizontal path when a direction of a linesegment of the polygonal line changes. Additionally, the computingsystem is configured to provide a horizontal path to an autopilot basedon the determinations.

In yet another embodiment, a computer program is embodied on anon-transitory computer-readable medium. The computer program configuredto cause at least one processor to analyze a basis profile to determineregions of positive and negative average slopes and eliminate dips thatare less than a predetermined distance by filling these dips. Thecomputer program is also configured to cause the at least one processorto approximate a resultant path by straight lines in an s-z plane thatdo not exceed maximum predetermined slope limitations, do not violateclearance requirements from a terrain profile, and do not deviate morethan a predetermined amount from the basis profile without dips. Thecomputer program is further configured to cause the at least oneprocessor to connect the straight lines using arcs having maximumpossible radii to minimize vertical accelerations while fulfillingvertical legality requirements to generate a vertical path.Additionally, the computer program is configured to cause the at leastone processor to provide the vertical path to an autopilot of arotorcraft to fly along the generated vertical path safely close to theterrain.

In still another embodiment, a computer-implemented method includesgenerating, by a computing system, a horizontal and vertical safe pathfor a rotorcraft. The computer-implemented method also includesdetermining, by the computing system, maximum speed limits for each arcand line segment of the horizontal and vertical paths. Thecomputer-implemented method further includes generating, by thecomputing system, a dynamic smoothing profile for the rotorcraft from astarting position to an end position. Additionally, thecomputer-implemented method includes providing the speed profile, by thecomputing system, to an autopilot of the rotorcraft to fly at speeds inaccordance with the speed profile from the starting position to the endposition.

In another embodiment, a computer-implemented method includesdetermining, by a computing system, a global nominal path andcalculating, by the computing system, a running grid with apredetermined size. The running grid includes a square terrain andweights database segment centered around a current position of arotorcraft based on sensor information. When a portion of the globalnominal path that passes through the running grid is not possible totraverse or sub-optimal, the computer-implemented method also includesdetermining a more optimal path through the running grid and modifying aflight path to include the more optimal path through the running grid.The computer-implemented method further includes providing the globalnominal path, by the computing system, to an autopilot to execute flightalong the global nominal path.

In yet another embodiment, a computer-implemented method includesdetermining a plurality of nodes, by a computing system, that arelocated at centers of terrain cells in a terrain map and calculating, bythe computing system, a cost of traversing a straight line arc from eachcell center to any of its respective 8-connected neighbors. Thecomputer-implemented method also includes generating, by the computingsystem, a minimum cost path through the terrain map based on the costsof the straight line arcs as a basis line for a rough flight pathfootprint for a rotorcraft on an x-y plane.

BRIEF DESCRIPTION OF THE DRAWINGS

In order that the advantages of certain embodiments of the inventionwill be readily understood, a more particular description of theinvention briefly described above will be rendered by reference tospecific embodiments that are illustrated in the appended drawings.While it should be understood that these drawings depict only typicalembodiments of the invention and are not therefore to be considered tobe limiting of its scope, the invention will be described and explainedwith additional specificity and detail through the use of theaccompanying drawings, in which:

FIG. 1 is a terrain map illustrating three flight path types for ahelicopter.

FIG. 2 is an architectural diagram illustrating a closed loop approachto NOE path planning, according to an embodiment of the presentinvention.

FIG. 3 is a terrain representation illustrating voxels, according to anembodiment of the present invention.

FIG. 4 is a terrain map illustrating running grids, according to anembodiment of the present invention.

FIG. 5 is a graph illustrating computing visible and hidden cells alonga line, according to an embodiment of the present invention.

FIG. 6 is a graph illustrating an approximation of the shortest path bystraight lines in the horizontal plane, according to an embodiment ofthe present invention.

FIG. 7 is a graph illustrating a polygonal approximation to the basisline with its “safe corridor,” according to an embodiment of the presentinvention.

FIG. 8 is a graph illustrating design of a first arc line that makes thenew path tangent to the instantaneous ground speed vector, according toan embodiment of the present invention.

FIG. 9 is a graph illustrating geometric smoothing in the horizontalplane, according to an embodiment of the present invention.

FIG. 10 is a graph illustrating construction of a basis vertical profilealong the horizontal smoothed path, according to an embodiment of thepresent invention.

FIG. 11 is a graph illustrating an approximation of the vertical basisprofile by straight lines, according to an embodiment of the presentinvention.

FIG. 12 is a graph illustrating geometric smoothing in the verticalplane along the x-y path footprint, according to an embodiment of thepresent invention.

FIG. 13 is a graph illustrating computation of a maximum speed boundsprofile, according to an embodiment of the present invention.

FIG. 14 is a graph illustrating maximum acceleration dynamic constraintsapplied to the speed bounds profile, according to an embodiment of thepresent invention.

FIG. 15 is a graph illustrating maximum deceleration dynamic constraintsapplied to the speed bounds profile, according to an embodiment of thepresent invention.

FIG. 16 is a graph illustrating a dynamic smoothing speed profile,according to an embodiment of the present invention.

FIG. 17 is a flowchart illustrating a process for determining a flightpath for a rotorcraft, according to an embodiment of the presentinvention.

FIG. 18 is a flowchart illustrating a process for performing horizontalpath smoothing, according to an embodiment of the present invention.

FIG. 19 is a flowchart illustrating a process for performing verticalpath smoothing, according to an embodiment of the present invention.

FIG. 20 is a flowchart illustrating a process for providing a speedprofile for a rotorcraft, according to an embodiment of the presentinvention.

FIG. 21 is a flowchart illustrating a process for determining a flightpath using running grids, according to an embodiment of the presentinvention.

FIG. 22 is a block diagram illustrating a computing system configured toperform flight path planning and control for a rotorcraft, according toan embodiment of the present invention.

DETAILED DESCRIPTION OF THE EMBODIMENTS

Some embodiments of the present invention pertain to a path planningapproach that is deterministic and guarantees a safe, smoothed, close tooptimal path, unlike the conventional approaches discussed above. Thisapproach may be a closed loop autonomous NOE solution that includes: (1)mapping of the environment; (2) optimal NOE trajectory computation; and(3) smoothing of the NOE trajectory. By employing such embodiments,safety of rotorcraft of any size, as well as safety of personnel andobjects on the ground, can be guaranteed for beyond line-of-sight(BLOS), regardless of whether the rotorcraft is piloted or unmanned. Asused herein, “rotorcraft” refers to any manned or unmanned rotorvehicle, as well as any vehicle capable of NOE flight. Such embodimentsare guaranteed to provide a safe flight corridor within the terrain thattakes into account aircraft dynamics and physical clearances. Someembodiments may also minimize exposure to threats. In certainembodiments, when sensor or terrain blockage occurs, the approach isguaranteed to ensure flare and hover.

In some embodiments, the path information may be provided to anautopilot of a rotorcraft to implement flight along the path at thedetermined trajectory and speeds. In certain embodiments, this pathinformation may be used to create a virtual display overlay for a smallunmanned rotorcraft having a camera point-of view interface, such as theParrot Bebop™ with Occulus Rift™. For manned systems, the pathinformation may also be displayed to the pilot and/or copilot via ascreen or helmet display to show the path. The autopilot may implementthe path, or the pilot may optionally fly the rotorcraft along the path.Such a display may show the path and maximum safe speed to the pilot orcontroller of the rotorcraft.

Some embodiments also provide for relative simplicity of implementation,integration, and testing. Sequential computations may be performed, andeach sequence/stage can be readily isolated and tested. Some embodimentsare also deterministic. The solution may be based on relatively lesscomplex and more efficient algorithms compared with conventionalapproaches. These algorithms may not be dependent on convergence orlearning of any system, as is required for neural nets, for example.

FIG. 2 is an architectural diagram illustrating a closed loop approachto NOE path planning 200, according to an embodiment of the presentinvention. NOE path planning approach 200 involves sensor mapping of theenvironment and incorporation into the onboard database to produce amapping of the environment 210, optimal NOE trajectory computation 220(performed globally before the mission, within a “running grid” aroundthe rotorcraft while flying), smoothing of the NOE trajectory 230according to rotorcraft flight characteristics, and rotorcraft platformdynamics, control, and sensing 240, which includes helicopter physicaldynamics 244, helicopter state sensors 246 (e.g., GPS, IMU, etc.), andautopilot 242. Sensor aiming logic 250 is provided in order to maximizesensor coverage and mapping over the planned path. Information is sharedamong the various elements of system 200 in order to provide a completeapproach that avoids obstacles and guarantees the optimal path.

Mapping of the environment 210 involves utilizing sensors 212 on therotorcraft. Sensors 212 in this embodiment gather millimeter wave (MMW),laser, vision, and electronic counter measures (ECM) data for sensorfusion. Sensors 212 also receive sensor aiming information for optimalmapping 252 from sensor aiming logic 250. One excellent fit for such asystem for a small UAS implementation would be the Velodyne LIDARVLP-16. The terrain database in this embodiment stores data on the nodesof a regular grid covering a rectangular portion of terrain alignedNorth-y and East-x. The terrain database provides a three-dimensionalvoxel (volumetric element)-based model of the terrain. An example ofthis database is represented as a vector of structures of the followingtype D_POINT (each structure representing a terrain grid node or“terrain cell”):

Father Structure:

typedef struct {  float h;  short flag;  struct _son *ptr; } D_POINT;Son Structure: typedef struct _son {  float h_low;  float h_high; struct _son *ptr_s; } SON;

The structure of each element in the vector contains the height of theterrain cell (h) in meters and a status flag (flag) that provides aconfidence of the height data. If flag=0, information on the respectiveterrain cell has not yet been updated by sensors. If flag>0, the valueindicates the level of confidence regarding the accuracy of the heightdata, where a higher number indicates a higher confidence.

A pointer to the SON structure (ptr) contains the height bounds of anobstacle “hanging over” the terrain cell, such as a bridge, wire, etc.ptr will be empty (null) for all terrain cells that do not containhanging obstacles, and ptr will have a non-null value and allocatedmemory for terrain cells that contain hanging obstacles.

When ptr is allocated, it contains the address of a SON structure thatstores the lowest and highest height of the first “layer” (or obstacle)and a null pointer ptr_s to another SON structure. If there are morelayers, the pointers will be allocated recursively to structurescontaining the minimum and maximum heights of these layers. In this way,memory is allocated only when necessary. If sensor data indicates thattwo layers are actually one, or the gap between them is too small forthe rotorcraft to safely fly between them, the upper height bound of thelower layer will be adjusted to the upper height bound of the adjacentlayer and the second layer's SON structure will be deallocated,releasing memory and taking care of the chain of subsequent pointersaccordingly.

The database may be initialized with the following values for allterrain cells: h=interpolated height according to an available coarseDTM, flag=0, and ptr=NULL. At each computation cycle, the informationfrom the whole sensor suite is merged and incorporated into the terraindatabase such that no information is lost. The height and confidencevalues of each scanned terrain cell are updated, and pointer links aremerged or built accordingly so that new data does not require extramemory. Therefore, the ANOE system in this embodiment “remembers”obstacles already seen and always finds a safe navigation path. Theweights associated with the terrain cells are stored in another vectorof same length as the terrain database vector (i.e., a weights database)so that the same index addresses the weight and terrain data of anycell. These weights are used by the optimal path computation algorithm.Each weight is computed as the percentage of cells within a certainradius that have LOS with the given cell. Therefore, higher weight meanshigher exposure to any potential threat, and usually it corresponds tohigher elevation.

The processed sensor information is then stored in a database via adatabase update 216. The database update is performed periodically aspart of the real-time processing loop of closed loop approach to NOEpath planning 200.

Optimal NOE trajectory computation 220 includes optimal path computation222. This involves determining the optimal path through the environmentbased on known terrain and obstacles. If an NOE path is not found at224, the running grid is enlarged and/or the height of the rotorcraft isincreased at 226 and step 224 is repeated.

If an NOE path is found at 226, smoothing of the NOE trajectory 230 isperformed for the path. This involves smoothing the path at 232 andcomputing the maximum speed profile (MSP) 234 according to vehicledynamics.

Helicopter model 240 includes various components pertaining to therotorcraft. Helicopter (plant) 244 is the physical rotorcraft that isbeing controlled in this embodiment. Autopilot 242 and state sensors 246are required avionics for the implementation, and many options arecommercially available for manned and unmanned rotorcraft. In thisembodiment, state sensors 246 include a global positioning system (GPS),inertial measurement unit (IMU), and accelerometers. Autopilot 242executes the nominal path generated by this embodiment and maintains therotorcraft position and speed within a small margin of error of thisnominal path, as measured by the IMU and GPS. The environment mappingsensors are aimed at the terrain under the pre-planned path in theproximity of the rotorcraft. Sensor aiming logic 250 takes into accountthe speed and acceleration of the rotorcraft and the geometry of thepre-planned flight path. Sensor aiming logic 250 performs sensor aiming252 by aiming the vision/laser/radar systems to specific areas withinthe scanned region that may cover layered terrain cells, obstacles(e.g., sudden rises in height), have insufficient “confidence levels”(i.e., haven't been scanned before or have been scanned only a fewtimes), and lie within the vicinity of the currently planned path. Therequirements and algorithms for sensor aiming are tailored for thespecifications of the sensors (e.g., field of view, etc).

In some embodiments, the terrain database stores data pertaining to thenodes of a regular grid covering a rectangular portion of terrainaligned North (y) and East (x). This may provide a three-dimensionalvoxel (volumetric element)-based model of the terrain, as can be seen interrain representation 300 of FIG. 3. The cell size may be chosen to beequal to two rotor diameters, but any desired cell size may be used.However, smaller cell sizes may add overhead into the calculations,particularly the “safety corridor” evaluation, and larger cell sizes areless optimal.

The terrain representation used in some embodiments is a two dimensionalarray of structures containing altitude information in a similar way toDTMs (i.e., an elevation array type of representation). However,overhang height information is also stored for each grid point in someembodiments, enabling true three-dimensional terrain representation, aswell as the weight associated with the grid point. The overhang heightinformation may be recomputed each time the terrain information isupdated within a certain range from the grid point. The data receivedfrom the sensors may be compressed and stored in the database todecrease the amount of memory that is needed.

Each terrain cell may be assigned a weight to be used by a shortestpath-planning algorithm, reflecting the vehicle's overall LOS exposureto the surrounding terrain. This is particularly useful for militaryoperations. The path planning process may be divided into two stages:(1) global path computation from the take-off position to the targetposition; and (2) in-flight path recomputation around the nominal globalpath, avoiding newly discovered obstacles. Global path computation maybe performed before take-off based on the coarse terrain data availableat the time. In-flight path recomputation may be performed on anaxis-aligned square section of the database called the running grid.Computations within a smaller area surrounding aircraft have been donebefore, but the application of the running grid in embodiments disclosedherein is novel. For instance, the way some embodiments can expand andstitch with the previous global solution in order to guarantee a pathhas never been performed before. The running grid is typically centeredon the present position of the vehicle, and may be of a size equal totwice the maximum range of the sensor suite, from the center of thissection to its point of intersection with the global path. A terrain map400 illustrating running grids 410, 412, and 414 at times t_(i),t_(i)+Δt, and t_(i)+2Δt, respectively, is shown in FIG. 4.

The running grid in this embodiment is a square terrain and weightsdatabase segment that is centered on the vehicle's present position. Therunning grid is oriented parallel to the x-y axis in this embodiment andits boundaries depend on the sensor suite's maximum range—its side sizeis twice this maximum range. When the navigation path has to berecomputed, the weights of terrain cells inside the running grid areupdated.

At each computation cycle, the information from the whole sensor suiteis merged and incorporated into the terrain database in someembodiments. As such, no information is lost. The height and“confidence” values of each terrain cell, assigned as a function ofsensor updates scanned, may be updated, and pointer links may be mergedor built accordingly so that new data does not require extra memory. Assuch, the system “remembers” obstacles that have already been detectedand always finds a safe navigation path.

The path generation process is sequential. The shortest path through theeight-connected grid graph from the local start to goal (i.e., points onthe nominal global path) may be generated by the modified Dijkstraalgorithm, providing a “basis line.” Geometric smoothing is applied tothis “basis line” to generate a smooth three-dimensional trajectory thatcan be realistically followed by the rotorcraft. Dynamic smoothing onthe three-dimensional trajectory provides the maximum possible speedprofile over the path, enabling an accurate time and fuel estimate(e.g., fossil fuel, battery power, or any other suitable fuel or power)for the specific rotorcraft mission.

Geometric smoothing generates a smooth, threat-and-collision-free “safe”three-dimensional flight path for the vehicle. The path may be computedup to a predetermined range from the present position or up to themaximum range to which there are reliable terrain database updates bythe sensors along the nominal path. The rotorcraft may be taken as abody in space and not as a point mass in order to account for itsprofile. As such, the nominal flight path may be taken as the centerlineof a rectangular cross-section “corridor” (or “tunnel”) in space, wherethe cross-section sizes are determined by the dimensions and clearancesnecessary for the specific vehicle.

Horizontal plane “legality” (or “safety”) of a nominal path line or arcrequires the legality of the centerline, the right boundary, and theleft boundary of the corridor. The “prices” over the three lines or arcsmay be determined by the same subroutines used in Dijkstra's algorithm.If any of the three prices exceeds the original optimal path's price bya certain amount or percentage, then the nominal line or arc may bedeemed “illegal” (or “unsafe”). Vertical plane “legality” requires thelegality of the centerline, the upper boundary, and the lower boundaryof the corridor, and may be determined by collision (or “contact”) withthe terrain profile. If any of the three lines (or arcs) intersects theterrain profile, then the nominal line or arc may be deemed illegal orunsafe.

Geometric smoothing may first involve generation of the smooth path inthe (x, y) plane. The optimal path may be approximated by the longestlegal straight lines, connected by the largest-radius legal tangentarcs. Each arc and line pair is called a horizontal geometrical unit(HGU).

Next, the smooth path may be generated in the (s, z) plane. Once thesmooth path is generated in the (x, y) plane, the coordinate “s” may bedefined along this path. A profile, or “cut,” of the terrain may begenerated along this path from terrain database height data, and aparallel profile at the nominal flight height may then be generatedabove the profile. A “lines-and-arcs approximation” to this secondprofile may be generated in the (s, z) plane by a different algorithmthan in the (x, y) plane. “Dips” or “dives” may be minimized while stillfollowing the terrain since rotorcraft usually have low safety marginsfor negative acceleration. This algorithm was designed by modeling theway helicopter pilots tend to “hug” the terrain, but not really followit, in NOE flight. The algorithm also minimizes the vertical controleffort of the autopilot. Each arc and line in the (s, z) smoothed pathis called a vertical geometrical unit (VGU). Dynamic smoothing requiresflight test data on the specific rotorcraft, including speed vs.required flare distance data, maximum bank angles, maximum accelerationprofiles, etc., and generates the maximum possible flight speed profileover the entire smooth trajectory.

The start and end points of the VGUs and HGUs along s may be determined,and the maximum allowable entry and exit speeds to the VGUs and HGUs maybe computed. The tighter the arc, the more slowly a helicopter or otherrotorcraft flies. Therefore, the speed at the end of the previous VGUshould not exceed the maximum entry speed of the next VGU. The moreinclined a VGU, the slower the rotorcraft needs to fly, constraining thespeed on that section. Many other dynamic constraints may also beconsidered, for example, flare distance versus speed, maximum ascendingand descending path angles versus speed, etc.

Speed at s=0 is the current speed vector, and speed at s=(clear pathlength) is generally taken as zero since the rotorcraft should becapable to complete flare and reach full hover at the end of thecomputed nominal path. Consequently, if any new object is detected alongthe path that obstructs the sensors' “view”, and a safe new path cannotbe found, the rotorcraft can still attain hover safely. In this case of“emergency hover,” the rotorcraft may start an ascending spiral toperform a broad sensor sweep until a new path is found. Once the boundsof the speed profile are determined, the maximum acceleration anddeceleration data for the rotorcraft may be used to compute the maximumphysically feasible speed profile, minimum time, and fuel requirements.This profile may also be used by the rotorcraft control system.

In order to achieve NOE behavior, it is assumed in some embodiments thatthreats can become suddenly active at any point on the terrain.Therefore, each terrain cell may be assigned a weight to be used by ashortest path planning algorithm, reflecting its overall LOS exposure tothe surrounding terrain. The less exposed terrain cell is, the moredesirable it is to fly over that cell. The weight of each terrain cellweight_(i) may be computed by visibility analysis of the surroundingterrain, up to a nominal radius (range) R_(nom), given by

$\begin{matrix}{{weight}_{i} = \frac{n_{LOS}}{n_{TOT}}} & (1)\end{matrix}$

where n_(LOS) is the number of cells within the radius that have LOSwith the center at nominal altitude above the ground, and n_(TOT) is thetotal number of cells within the nominal radius. This concept isillustrated in graph 500 of FIG. 5. The visibility of each grid cell 510along a given line 520 from a viewing point 530 can be determined bycomputing an elevation angle 512 from viewing point 530 to each cell510, increasing the range, and comparing the elevation angle with themaximal angle among all cells closer to the viewpoint. If the angle isgreater than the maximal angle among all cells before it, then this cellis visible.

The circular region for weight computation around each cell may besubdivided into equal sectors so that each sector is at most one terraincell wide at maximum range, given by

$\begin{matrix}{\theta_{sector} \cong {{arc}\; {\tan ( \frac{d}{R_{nom}} )}}} & (2)\end{matrix}$

where d is the terrain cell size. Visibility can be computed along thecenterline of each sector, such that every terrain cell inside theregion is evaluated at least once and tagged as “visible” (having LOSwith the center cell) or “hidden” (no LOS with the center cell). Thus,n_(Los) is the number of “visible” cells and n_(TOT) is the number ofvisible cells plus the number of hidden cells.

Coarse initial terrain information may be assumed to be good enough toidentify the main features of the terrain (e.g., hills, passes, canyons,etc.), but devoid of sufficient detail pertaining to obstacles close tothe ground (e.g., trees, poles, antennas, wires, etc.). Therefore, itmay be assumed that the globally optimal path computed with initialterrain information, and a globally optimal path computed having exactterrain information, would be roughly similar. As a consequence, localin-flight path recomputations within the running grid would yield aquasi-optimal path. In the worst case scenario, where an extremely largeobstacle is detected on the path and running grid path recomputationsfail, the running grid may be enlarged successively until a new path isfound. The resulting path plan is not globally optimal, but it is likelythe best possible solution given the in-flight recomputation constraints(e.g., short computation time, limited computational resources and fuel,and aircraft safety).

The shortest path planning algorithm may be a variation of Dijlstra'salgorithm, where the nodes are located at the centers of the terraincells and the cost of traversing a straight line arc from a cell to anyof its 8-connected neighbors is given by

$\begin{matrix}{{\Delta \; C_{i\mspace{14mu} {to}\mspace{14mu} j}} = {\frac{l_{ij}}{d}( {w_{i} + w_{j}} )}} & (3)\end{matrix}$

where d is the cell size, w_(i) and w_(j) are the weights of cells i andj, respectively, and l_(ij) is the distance between the terrain cells'centers. l_(ij) may be computed as

l _(ij)=√{square root over ((h _(i) −h _(j))²+(x _(i) −x _(j))²+(y _(i)−y _(j))²)}  (4)

where (x_(i), y_(i)) and (x_(j), y_(j)) are the coordinates of cells iand j, respectively, and h_(i) and h_(j) are their respective heights.For diagonal neighbors, the cost of the arc yields

$\begin{matrix}{{\Delta \; C_{{i\mspace{14mu} {to}\mspace{14mu} j},{diag}}} = {\sqrt{2 + \frac{( {h_{i} - h_{j}} )^{2}}{d^{2}}}\mspace{11mu} ( {w_{i} + w_{j}} )}} & (5)\end{matrix}$

and for 4-connected (i.e., up, down, left, and right) neighbors,

$\begin{matrix}{{\Delta \; C_{{i\mspace{14mu} {to}\mspace{14mu} j},{4C}}} = {\sqrt{1 + \frac{( {h_{i} - h_{j}} )^{2}}{d^{2}}}\; ( {w_{i} + w_{j}} )}} & (6)\end{matrix}$

In order to accelerate the computation speed, l_(ij) ² can be used forthe arc cost instead of l_(ij) to eliminate the need for relativelyexpensive square root computations. Moreover, if the terrain is not verysteep, such that the difference in heights between adjacent cells ismuch smaller than the cell size (e.g., a difference of an order ofmagnitude), then the arcs can be computed as

ΔC _(i to j,diag)=√{square root over (2)}(w _(i) +w _(j))  (7)

ΔC _(i to j,4C)=(w _(i) +w _(j))  (8)

This admissible heuristic allows for the minimization of path cost anddistance at the same time, and the path found between start and targetpoints is globally quasi-optimal within the computation domain.

The shortest path through the eight-connected grid graph from the localstart to goal (i.e., points on the nominal global path) generated by themodified Dijkstra algorithm does not represent a valid flight path.Rather, it yields a rough design of the flight path's footprint on thex-y plane. See terrain representation 300 of FIG. 3. By analyzing thefootprint, taking into account the present state of the rotorcraft andits flight envelope, the three-dimensional flight path for therotorcraft may be generated.

Horizontal Path Generation

A polygonal line (i.e., basis line) connecting the centers of theeight-connected cells on the shortest path is the basis for this lateralpath. Its “cost” may be determined analogously to the costs in themodified Dijkstra path planning algorithm. This polygonal line maysubsequently be approximated by the minimum possible number of straightlines between points on the basis path so that the new path cost doesnot exceed the basis path's cost by more than a given percentage. Seegraph 600 of FIG. 6.

The cost analysis takes into account a “corridor” around the nominalcenterline. The width of the corridor is equal to two rotor diameters inthis embodiment, ensuring the safety of the aircraft throughout itslength while not making the corridor unnecessarily large. See graph 700of FIG. 7.

The horizontal path in this embodiment is designed to start at thepresent position, aligned with or tangent to the velocity vector. Assuch, if the first line is not aligned with the velocity vector, an arcthat is tangent to this vector may be designated as the first HGU, andthe first line may be modified to also be tangent to this arc.

According to the equation for centripetal acceleration,

$\begin{matrix}{a = \frac{v^{2}}{R}} & (9)\end{matrix}$

where a is acceleration, v is velocity, and R is the arc's radius. Rshould be designed to be larger than or equal to

$\begin{matrix}{R_{\min} = \frac{v_{tg}^{2}}{a_{\max,{LAT}}}} & (10)\end{matrix}$

where a_(max,LAT) is the maximum lateral load factor and v_(tg) is thetangent velocity. See graph 800 of FIG. 8. Note that small,state-of-the-art, multi-rotor UASs are capable of performing very tight,uncoordinated maneuvers that violate Equation 10. However, suchmaneuvers may be unsafe and/or energy-wasteful, and they may result inreduced sensor updates on the terrain.

Subsequently, the lines approximating the basis path are connected withtangent arcs. The arcs are designed to have the maximum possible radiisince small radii imply low speeds for the maximum possible lateralacceleration, which is a dynamic constraint of the rotorcraft. However,the resultant path is still required to comply with the cost and“legality” requirements mentioned above. A connecting arc with a radiusof half the terrain cell size is always legal since the cell size ischosen to be equal to two rotor diameters in this embodiment. Thus, thealgorithm connecting the lines is always successful. Also, a legal linecan always be found between two neighboring terrain cells. Therefore,the horizontal path generation algorithms are successful and yield asmooth flight path footprint composed of lines and arcs (i.e., theHGUs). See graph 900 of FIG. 9.

Vertical Path Smoothing

The basis profile may be analyzed to determine regions of positive andnegative average slopes. The points where the slope changes frompositive to negative are called “peaks.” The regions between peaks arecalled “dips.” In order to minimize dives, the dips that are too narrowmay be eliminated by covering them as if they were filled with water.See graph 1000 of FIG. 10.

The resultant path may then be approximated by straight lines in the s-zplane that do not exceed maximum slope limitations, do not violateclearance requirements from the terrain profile, and do not deviate toomuch from the basis nominal profile without dips. See graph 1100 of FIG.11. The amount of deviation that is permitted may be programmable anddetermine behaviors of the system that can be tailored to specificmissions/platforms. Arcs may then be used to connect the resultinglines, having the maximum possible radii to minimize verticalaccelerations while still fulfilling the “vertical legality′”requirements mentioned above. See graph 1200 of FIG. 12. Tangency of thespeed vector to the path in the vertical plane may be achieved through aconnecting arc of a radius greater than or equal to R_(min) applied tothe maximum allowed vertical centripetal acceleration and the speedvalue.

Dynamic Smoothing

The generation of a maximum ground speed profile for a given rotorcraftflying along the smooth path described above as a function of s iscalled “dynamic smoothing.” Dynamic smoothing is a function of themaximum distance along the path traversed over terrain cells that havebeen scanned by the sensors D_clear. The heights of the cells are knownwith a high degree of certainty. Minimum-time, maximum-fuel-savingtrajectory planning is based on dynamic smoothing. This approach totrajectory planning is unique to embodiments described herein.

In this approach, the ground speed at s=0 (i.e., the present position)in dynamic smoothing analysis should be the present (i.e.,instantaneous) speed, and the speed at s=D_clear is taken as zero toensure survivability even in the worst case scenario where no new pathhas been sufficiently scanned by the sensors and a large obstacle blocksthe current path. Path recomputation is generally required when D_clearfalls below a certain threshold. In this case, the running grid size maybe increased in an attempt to find an alternate solution around theobstacle. In the worst-case scenario, flare may be commanded along thecurrent safe path. The rotorcraft may achieve hover if its path isblocked by a large obstacle, and enlarging the running grid up to apredetermined size also does not yield a safe path. The rotorcraft maythen initiate a rising spiral maneuver, scanning its surroundings withthe sensors until a new safe path is found. If sufficient sensors arepresent on the rotorcraft, alternatively, the rotorcraft may simply riseto a height sufficient to discover a new safe path.

Once the horizontal and the vertical path have been generated, maximumspeed limits can be determined for each arc and line in the horizontaland vertical planes. Also, there is generally a maximum allowed speedfor NOE flight, specified by mission guidelines, that is not to beexceeded even in cases where the dynamic constraints would permit veryhigh speeds. In the vertical plane, for straight lines, the slopedetermines the maximum allowed speed. For arcs, the radius R and maximumallowed vertical acceleration a for push-downs and pull-ups willdetermine the maximum speed v_max. For vertical maneuvers, the maximumspeed may be roughly approximated by

v _(max push-down)=√{square root over (a _(max push-down) *R)}  (11)

v _(max pull-up)=√{square root over (a _(max pull-up) *R)}  (12)

The “speed bounds graph” thus generated becomes the “bounding box” forthe dynamic smoothing final graph. See graph 1300 of FIG. 13.

In order to generate the dynamic smoothing final profile, theacceleration curves for the rotorcraft are applied at each point of thespeed bounds graph where there is an increase (i.e., step up) in speed,beginning from the current speed at s=0. See graph 1400 of FIG. 14. Theflare (deceleration) curves are then applied backwards from s=0 tos=D_clear at each point of the speed bounds graph where there is afalling step in speed, beginning from zero speed at s=D_clear. See graph1500 of FIG. 15. The dynamic smoothing final profile is obtained fromthe minimum of the values of the bounds, acceleration, and decelerationgraphs. The final profile represents the maximum speed profilephysically possible for the given rotorcraft, and allows precise fuel(worst case) and time (best case) trajectory computations. See graph1600 of FIG. 16.

FIG. 17 is a flowchart 1700 illustrating a process for determining aflight path for a rotorcraft, according to an embodiment of the presentinvention. The process begins with updating cell heights and weightsaccording to sensor input at 1705. The cells are determined a-priori,and the cell information may then change based on sensor information. Insome embodiments, a weight may be assigned to each of the cells, whereinthe weight reflects overall LOS exposure to terrain surrounding eachcell. In certain embodiments, the height and weight for a cell may berecomputed when the cell's height changes more than a certain percentagebased on sensor information. The grid area for recomputation may then bedetermined at 1710 (i.e., the size of the running grid).

A shortest safe path is then calculated through the grid graph from alocal start to a local goal defined as points on a nominal global pathat 1715. The calculating of the shortest safe path may includedetermining a corridor including a cross-section that takes into accountdimensions and clearances for the rotorcraft. In some embodiments, thecalculation of the shortest path may also include determining horizontalplane legality of the nominal path, a right boundary, and a leftboundary of the corridor. In certain embodiments, the calculation of theshortest path may include determining vertical plane legality of thenominal path, an upper boundary, and a lower boundary of the corridor.

Geometric smoothing is then performed on a basis line from the localstart to the local goal to generate a smooth three-dimensionaltrajectory that can be followed by the rotorcraft at 1720. The geometricsmoothing may include generating a smooth path in an x-y plane. In someembodiments, an optimal path may be approximated by longest legalstraight lines connected by largest radius legal tangent arcs, whereeach arc and line form a HGU. In certain embodiments, the geometricsmoothing may include generating a smooth path in an s-z plane. In someembodiments, a quasi-optimal path may be approximated by minimizingdives while following terrain, where each arc and line in the smoothedpath form a VGU.

Dynamic smoothing is performed on the three-dimensional trajectory at1725 to provide a maximum possible speed profile over a path defined bythe dynamic smoothing. The dynamic smoothing may include determining HGUand VGU start and end points along s in an s-z plane and calculatingmaximum allowable entry and exit speeds for the start and end points. Insome embodiments, speed at s=0 is a current speed vector and speed ats=(clear path length) is taken as zero to ensure that the rotorcraft iscapable of completing flare and reaching full hover at an end of thecleared nominal path.

If the path is blocked at 1730, the system enlarges the running grid inan attempt to find a solution at 1735. In some embodiments, the runninggrid may be enlarged at most to a predetermined maximum size. If thepath is still blocked after enlarging the running grid, an ascent orascending spiral of the rotorcraft is initiated and a sensor sweep isperformed at 1740 until a new path is detected where the obstacle is nolonger blocking, and the process returns to 1715. If the path is notblocked after enlarging the running grid at 1735, the process alsoreturns to 1715. Geometric smoothing and dynamic smoothing are againperformed. If the path is clear, a nominal path and speed profile isprovided at 1745 to be used by an autopilot to control the rotorcraft tofly along the defined path.

FIG. 18 is a flowchart 1800 illustrating a process for performinghorizontal path smoothing, according to an embodiment of the presentinvention. A basis line through points at centers of 8-connected cellsis determined on a modified Dijkstra shortest path through a grid graphfrom a start point to an end point at 1810. A polygonal line defining aminimum possible number of straight lines between points on the basisline and its parallel boundary lines are then determined at 1820. Thepolygonal line connects the start point and the end point. The boundarylines are part of the criteria for choosing the polygonal line. Theboundary lines of the corridor are determined by dimensions andclearances for the rotorcraft. Legality of the polygonal line and theboundary lines may also be determined. When a first line of thepolygonal line is not aligned with a velocity vector of the rotorcraftat 1830, an arc is designated at 1840 that is tangent to the velocityvector as a first horizontal geometrical unit, and the first line of thepolygonal line is modified at 1850 to be tangent to the arc.

When a direction of a line segment of the polygonal line changes, aconnecting arc is determined at 1860 for each change in direction togenerate a horizontal path. In some embodiments, connecting arcs have aradius of half a size of the 8-connected cells. A horizontal path isprovided to an autopilot at 1870 based on the determinations, which maythen control the rotorcraft based on the horizontal path.

FIG. 19 is a flowchart 1900 illustrating a process for performingvertical path smoothing, according to an embodiment of the presentinvention. A basis profile is analyzed at 1910 to determine regions ofpositive and negative average slopes. The basis profile may follow asmooth path in an x-y plane. Dips that are less than a predetermineddistance are then eliminated by filling these dips at 1920.

A resultant path is approximated at 1930 by straight lines in an s-zplane that do not exceed maximum predetermined slope limitations, do notviolate clearance requirements from a terrain profile, and do notdeviate more than a predetermined amount from the basis profile withoutdips. The straight lines are connected at 1940 using arcs having maximumpossible radii to minimize vertical accelerations while fulfillingvertical legality requirements to generate a vertical path. Theautopilot of the rotorcraft is then provided with the vertical path tocontrol the rotorcraft to fly along the generated vertical path at 1950.

FIG. 20 is a flowchart 2000 illustrating a process for providing a speedprofile for a rotorcraft, according to an embodiment of the presentinvention. A horizontal and vertical safe path are generated for therotorcraft at 2010. The generating of the horizontal and vertical safepath may include generating a smooth path in an x-y plane and an s-zplane, respectively. Maximum speed limits are determined for each arcand line segment of the horizontal and vertical paths at 2020.

A dynamic smoothing profile is generated for the rotorcraft from astarting position to an end position at 2030. The dynamic smoothingprofile generation may include determining horizontal safe path andvertical safe path start and end points along s in an s-z plane andcalculating maximum allowable entry and exit speeds for the start andend points. Dynamic smoothing generates the speed profile. The speedprofile is then provided to an autopilot at 2040 and the rotorcraft iscontrolled by the autopilot to fly at speeds in accordance with thespeed profile from the starting position to the end position.

FIG. 21 is a flowchart 2100 illustrating a process for determining aflight path using running grids, according to an embodiment of thepresent invention. A global nominal path is determined a-priori beforeflight or on power-up at 2110. A running grid is calculated with apredetermined size at 2120. The running grid includes a square terrainand weights database segment centered around a current position of arotorcraft based on sensor information. The size of the running grid maybe twice the range of the sensor suite of the rotorcraft in someembodiments.

When a portion of the global nominal path that passes through therunning grid is not possible to traverse or sub-optimal, a more optimalpath through the running grid is determined and the flight path ismodified at 2130 to include the more optimal path through the runninggrid. When impassable, in some embodiments, the running grid may beenlarged up to a predetermined size, and/or an ascent or ascendingspiral of the rotorcraft may be initiated and a sensor sweep may beperformed. The smoothed path and speed profile is then provided to anautopilot to execute flight along the global nominal path at 2140. Therunning grid may be periodically recomputed and the process may berepeated if flight is not finished at 2150.

FIG. 22 is a block diagram illustrating a computing system configured toperform flight path planning and control for a rotorcraft, according toan embodiment of the present invention. In some embodiments, computingsystem 2200 may execute the processes of FIGS. 17-21. System 2200includes a bus 2205 or other communication mechanism for communicatinginformation, and processor(s) 2210 coupled to bus 2205 for processinginformation. Processor(s) 2210 may be any type of general or specificpurpose processor, including a central processing unit (CPU) orapplication specific integrated circuit (ASIC). System 2200 furtherincludes a memory 2215 for storing information and instructions to beexecuted by processor(s) 2210, as well as storing the height and weightsof the terrain database. Memory 2215 can be comprised of any combinationof random access memory (“RAM”), read only memory (“ROM”), flash memory,cache, static storage such as a magnetic or optical disk, or any othertypes of non-transitory computer-readable media or combinations thereof.Additionally, system 2200 includes a communication device 2220, such asa transceiver, to facilitate wired or wireless communication withexternal systems (e.g., autopilot, although this may be part of the samesystem in some embodiments).

Non-transitory computer-readable media may be any available media thatcan be accessed by processor(s) 2210 and may include both volatile andnon-volatile media, removable and non-removable media, and communicationmedia. Communication media may include computer-readable instructions,data structures, program modules or other data in a modulated datasignal such as a carrier wave or other transport mechanism and includesany information delivery media.

Memory 2215 stores software modules that provide functionality whenexecuted by processor(s) 2210. The modules include an operating system2225 for system 2200. The modules further include a flight path planningand control module 2230 that is configured to determine a flight pathfor the rotorcraft and control flight. For instance, flight pathplanning and control module 2230 may be configured to perform any of theflight path planning and control operations discussed herein. System2200 may include one or more additional functional modules 2235 thatinclude additional functionality. In some embodiments, the additionalfunctional modules may include a graphics overlay module that enablesdisplaying of a virtual display overlay (i.e., an “electronic tunnel”)via a display (not shown). Sensors 2240 provide information pertainingto the local environment, such as terrain and obstacles, around therotorcraft.

One skilled in the art will appreciate that a “system” could be embodiedas a computer, a server, a console, or any other suitable computingdevice, or combination of devices. Presenting the above-describedfunctions as being performed by a “system” is not intended to limit thescope of the present invention in any way, but is intended to provideone example of many embodiments of the present invention. Indeed,methods, systems and apparatuses disclosed herein may be implemented inlocalized and distributed forms consistent with computing technology,including cloud computing systems.

It should be noted that some of the system features described in thisspecification have been presented as modules, in order to moreparticularly emphasize their implementation independence. For example, amodule may be implemented as a hardware circuit comprising custom verylarge scale integration (VLSI) circuits or gate arrays, off-the-shelfsemiconductors such as logic chips, transistors, or other discretecomponents. A module may also be implemented in programmable hardwaredevices such as field programmable gate arrays, programmable arraylogic, programmable logic devices, graphics processing units, or thelike.

A module may also be at least partially implemented in software forexecution by various types of processors. An identified unit ofexecutable code may, for instance, comprise one or more physical orlogical blocks of computer instructions that may, for instance, beorganized as an object, procedure, or function. Nevertheless, theexecutables of an identified module need not be physically locatedtogether, but may comprise disparate instructions stored in differentlocations which, when joined logically together, comprise the module andachieve the stated purpose for the module. Further, modules may bestored on a computer-readable medium, which may be, for instance, a harddisk drive, flash device, RAM, tape, or any other such medium used tostore data.

Indeed, a module of executable code could be a single instruction, ormany instructions, and may even be distributed over several differentcode segments, among different programs, and across several memorydevices. Similarly, operational data may be identified and illustratedherein within modules, and may be embodied in any suitable form andorganized within any suitable type of data structure. The operationaldata may be collected as a single data set, or may be distributed overdifferent locations including over different storage devices, and mayexist, at least partially, merely as electronic signals on a system ornetwork.

It will be readily understood that the components of various embodimentsof the present invention, as generally described and illustrated in thefigures herein, may be arranged and designed in a wide variety ofdifferent configurations. Thus, the detailed description of theembodiments of the systems, apparatuses, methods, and computer programsof the present invention, as represented in the attached figures, is notintended to limit the scope of the invention as claimed, but is merelyrepresentative of selected embodiments of the invention.

The features, structures, or characteristics of the invention describedthroughout this specification may be combined in any suitable manner inone or more embodiments. For example, reference throughout thisspecification to “certain embodiments,” “some embodiments,” or similarlanguage means that a particular feature, structure, or characteristicdescribed in connection with the embodiment is included in at least oneembodiment of the present invention. Thus, appearances of the phrases“in certain embodiments,” “in some embodiment,” “in other embodiments,”or similar language throughout this specification do not necessarily allrefer to the same group of embodiments and the described features,structures, or characteristics may be combined in any suitable manner inone or more embodiments.

It should be noted that reference throughout this specification tofeatures, advantages, or similar language does not imply that all of thefeatures and advantages that may be realized with the present inventionshould be or are in any single embodiment of the invention. Rather,language referring to the features and advantages is understood to meanthat a specific feature, advantage, or characteristic described inconnection with an embodiment is included in at least one embodiment ofthe present invention. Thus, discussion of the features and advantages,and similar language, throughout this specification may, but do notnecessarily, refer to the same embodiment.

Furthermore, the described features, advantages, and characteristics ofthe invention may be combined in any suitable manner in one or moreembodiments. One skilled in the relevant art will recognize that theinvention can be practiced without one or more of the specific featuresor advantages of a particular embodiment. In other instances, additionalfeatures and advantages may be recognized in certain embodiments thatmay not be present in all embodiments of the invention.

One having ordinary skill in the art will readily understand that theinvention as discussed above may be practiced with steps in a differentorder, and/or with hardware elements in configurations which aredifferent than those which are disclosed. Therefore, although theinvention has been described based upon these preferred embodiments, itwould be apparent to those of skill in the art that certainmodifications, variations, and alternative constructions would beapparent, while remaining within the spirit and scope of the invention.In order to determine the metes and bounds of the invention, therefore,reference should be made to the appended claims.

1. A computer-implemented method, comprising: determining a-priori, by acomputing system, a plurality of three-dimensional voxels as cells of arectangular grid, the cells having a predetermined length and width;calculating, by the computing system, a shortest safe path through thegrid graph from a local start to a local goal defined as points on anominal global path; performing, by the computing system, geometricsmoothing on a basis line from the local start to the local goal togenerate a smooth three-dimensional trajectory that can be followed by agiven rotorcraft; performing dynamic smoothing on the three-dimensionaltrajectory, by the computing system, to provide a maximum possible speedprofile over a path defined by the dynamic smoothing; and providing athree-dimensional path and speed profile, by the computing system, to beused by an autopilot to control the rotorcraft to fly along the definedpath.
 2. The computer-implemented method of claim 1, wherein thecalculation of the shortest safe path comprises determining a corridorcomprising a cross-section that takes into account dimensions andclearances for the rotorcraft.
 3. The computer-implemented method ofclaim 2, wherein the calculation of the shortest path comprisesdetermining horizontal plane legality of the nominal path, a rightboundary, and a left boundary of the corridor.
 4. Thecomputer-implemented method of claim 2, wherein the calculation of theshortest safe path comprises determining vertical plane legality of thenominal path, an upper boundary, and a lower boundary of the corridor.5. The computer-implemented method of claim 1, wherein the geometricsmoothing comprises generating a smooth path in an x-y plane.
 6. Thecomputer-implemented method of claim 5, wherein an optimal path isapproximated by longest legal straight lines connected by largest radiuslegal tangent arcs, each arc and line forming a horizontal geometricalunit (HGU).
 7. The computer-implemented method of claim 1, wherein thegeometric smoothing comprises generating a smooth path in an s-z plane.8. The computer-implemented method of claim 7, wherein a quasi-optimalpath is approximated by minimizing dives while following terrain, eacharc and line in the smoothed path forming a vertical geometrical unit(VGU).
 9. The computer-implemented method of claim 1, wherein thedynamic smoothing comprises determining HGU and VGU start and end pointsalong s in an s-z plane and calculating maximum allowable entry and exitspeeds for the start and end points.
 10. The computer-implemented methodof claim 9, wherein speed at s=0 is a current speed vector and speed ats=(clear path length) is taken as zero to ensure that the rotorcraft iscapable of completing flare and reaching full hover at an end of thecomputed nominal path.
 11. The computer-implemented method of claim 1,further comprising: when no safe path is found, enlarging a runninggrid, by the computing system, and searching for a safe path.
 12. Thecomputer-implemented method of claim 1, further comprising: when no safepath is found, initiating an ascent or ascending spiral of therotorcraft, by the computing system, and performing a sensor sweep whenan object is detected along the path that obstructs the sensors.
 13. Thecomputer-implemented method of claim 12, further comprising: detecting,by the computing system, a new path that is clear of obstacles; andinitiating flight of the rotorcraft, by the computing system, along thenewly detected path.
 14. The computer-implemented, method of claim 1,further comprising: assigning a weight, by the computing system, to eachof the cells, wherein the weight reflects overall line-of-sight (LOS)exposure to terrain surrounding each cell.
 15. The computer-implementedmethod of claim 14, wherein the weight weight_(i) is computed byvisibility analysis of surrounding terrain, up to a nominal radius(range) R_(nom), given by ${weight}_{i} = \frac{n_{LOS}}{n_{TOT}}$ wheren_(LOS) is a number of cells within R_(nom) that have LOS with a centerof a given cell at nominal altitude above the ground, and n_(TOT) is atotal number of cells within R_(nom).
 16. The computer-implementedmethod of claim 15, wherein determining the weight for a given cellfurther comprises subdividing a circular region of a maximum range dinto equal sector such that each sector is at most one cell wide,defined by$\theta_{sector} \cong {{arc}\; {\tan ( \frac{d}{R_{nom}} )}}$where visibility is computed along a centerline of each sector and eachcell is tagged as being visible or hidden.
 17. The computer-implementedmethod of claim 1, further comprising: receiving sensor input, by thecomputing system, regarding cell heights and weights; and when a cell'sheight changes by more than a predetermined percentage, recomputing thecell height and weight, by the computing system.
 18. Thecomputer-implemented method of claim 1, wherein the plurality ofthree-dimensional voxels further comprise cell weight and a pointerstructure storing information on overhangs.
 19. The computer-implementedmethod of claim 1, further comprising: providing the three-dimensionalpath and speed profile, by the computing system, to a graphics processorto overlay with a camera view to provide visual cues to a pilot tofollow a safe path.
 20. A computing system, comprising: memory storingexecutable computer program code; and at least one processor configuredto execute the computer program code, the at least one processorconfigured to: determine a basis line through points at centers of8-connected cells on a modified Dijkstra shortest path through a gridgraph from a start point to an end point, determine a polygonal linedefining a minimum possible number of straight lines between points onthe basis line, the polygonal line connecting the start point and theend point, determine boundary lines parallel to the polygonal linedefining a safe corridor, wherein the boundary lines of the corridor aredetermined by dimensions and clearances for a rotorcraft, when adirection of a line segment of the polygonal line changes, determine aconnecting arc for each change in direction to generate a horizontalpath, and provide a horizontal path to an autopilot based on thedeterminations.
 21. The computing system of claim 20, wherein when afirst line of the polygonal line is not aligned with a velocity vectorof the rotorcraft, the at least one processor is further configured to:designate an arc that is tangent to the velocity vector as a firsthorizontal geometrical unit; and modify the first line of the polygonalline to be tangent to the arc.
 22. The computing system of claim 20,wherein the connecting arc is greater than or equal to$R_{\min} = \frac{v_{tg}^{2}}{a_{\max,{LAT}}}$ where R_(min) is aminimum radius, a_(max,LAT) is a maximum lateral load factor, and v_(tg)is a tangent velocity.
 23. The computing system of claim 20, wherein theconnecting arcs have a radius of half a size of the 8-connected cells.24. The computing system of claim 20, wherein the computer program codeis further configured to cause the at least one processor to determinelegality of the polygonal line and the boundary lines.
 25. The computingsystem of claim 20, wherein the autopilot is configured to control therotorcraft based on the provided horizontal path.
 26. A computer programembodied on a non-transitory computer-readable medium, the computerprogram configured to cause at least one processor to: analyze a basisprofile to determine regions of positive and negative average slopes;eliminate dips that are less than a predetermined distance by fillingthese dips; approximate a resultant path by straight lines in an s-zplane that do not exceed maximum predetermined slope limitations, do notviolate clearance requirements from a terrain profile, and do notdeviate more than a predetermined amount from the basis profile withoutdips; connect the straight lines using arcs having maximum possibleradii to minimize vertical accelerations while fulfilling verticallegality requirements to generate a vertical path; and provide thevertical path to an autopilot of a rotorcraft to fly along the generatedvertical path safely close to the terrain.
 27. The computer program ofclaim 26, wherein the basis profile follows a smooth path in an x-yplane.
 28. A computer-implemented method, comprising: generating, by acomputing system, a horizontal and vertical safe path for a rotorcraft;determining, by the computing system, maximum speed limits for each arcand line segment of the horizontal and vertical paths; generating, bythe computing system, a dynamic smoothing profile for the rotorcraftfrom a starting position to an end position; and providing the speedprofile, by the computing system, to an autopilot of the rotorcraft tofly at speeds in accordance with the speed profile from the startingposition to the end position.
 29. The computer-implemented method ofclaim 28, wherein the generating of the horizontal and vertical safepath comprises generating a smooth path in an x-y plane and a s-z plane,respectively.
 30. The computer-implemented method of claim 28, whereinthe dynamic smoothing profile generation comprises determininghorizontal safe path and vertical safe path start and end points along sin an s-z plane and calculating maximum allowable entry and exit speedsfor the start and end points.
 31. The computer-implemented method ofclaim 28, further comprising: providing a three-dimensional path and thespeed profile, by the computing system, to a graphics processor tooverlay with a camera view to provide visual cues to a pilot to follow asafe path.
 32. A computer-implemented method, comprising: determining,by a computing system, a global nominal path; calculating, by thecomputing system, a running grid with a predetermined size, wherein therunning grid comprises a square terrain and weights database segmentcentered around a current position of a rotorcraft based on sensorinformation; when a portion of the global nominal path that passesthrough the running grid is not possible to traverse or sub-optimal,determining a more optimal path through the running grid and modifying athree dimensional flight path to include the more optimal path throughthe running grid; and providing the three dimensional flight path, bythe computing system, to an autopilot to execute flight along the globalnominal path.
 33. The computer-implemented method of claim 32, furthercomprising: initiating an ascent or ascending spiral of the rotorcraft,by the computing system, and performing a sensor sweep when the portionof the global nominal path is not possible to traverse.
 34. Thecomputer-implemented method of claim 32, wherein a size of each side ofthe running grid is twice a range of a sensor suite of the rotorcraft.35. The computer-implemented method of claim 34, further comprising:periodically recomputing the running grid, by the computing system, andmodifying the three dimensional flight path when the correspondingportion of the global nominal path that passes through the running gridis not possible to traverse or sub-optimal.
 36. The computer-implementedmethod of claim 32, further comprising: providing the three-dimensionalflight path and a speed profile, by the computing system, to a graphicsprocessor to overlay with a camera view to provide visual cues to apilot to follow a safe path.
 37. A computer-implemented method,comprising: determining a plurality of nodes, by a computing system,that are located at centers of terrain cells in a terrain map;calculating, by the computing system, a cost of traversing a straightline arc from each cell to any of its respective 8-connected neighbors;and generating, by the computing system, a minimum cost path through theterrain map based on the costs of the straight line arcs as a roughflight path footprint for a rotorcraft on an x-y plane.
 38. Thecomputer-implemented method of claim 37, wherein the cost of traversingeach straight line arc is computed as${\Delta \; C_{i\mspace{14mu} {to}\mspace{14mu} j}} = {\frac{l_{ij}}{d}( {w_{i} + w_{j}} )}$where d is a cell size, w_(i) and w_(j) are weights of cells i and j,respectively, and l_(ij) is a distance between the terrain cells'surfaces.
 39. The computer-implemented method of claim 38, whereinl_(ij) is computed asl _(ij)=√{square root over ((h _(i) −h _(j))²+(x _(i) −x _(j))²+(y _(i)−y _(j))²)} where (x_(i), y_(i)) and (x_(j), y_(j)) are coordinates ofcells i and j, respectively, and h_(i) and h_(j) are their respectiveheights.
 40. The computer-implemented method of claim 38, wherein when adifference in heights between adjacent terrain cells is an order ofmagnitude or more smaller than terrain cell size, arcs are computed asΔC _(i to j,diag)=√{square root over (2)}(w _(i) +w _(j))ΔC _(i to j,4C)=(w _(i) +w _(j)). where ΔC_(i to j,diag) represents adiagonal arc and ΔC_(i to j,4C) represents a 4-connected arc.